/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef VEHICLEJOGCONTROL_H
#define VEHICLEJOGCONTROL_H

#include "vehiclekinematics.h"
#include "vehicledefine.h"
#include <QString>
#include <vector>
#include "globalDefine.h"

class AxisObject;
class FilteringAlgorithm;
class VehicleVelocitySmoother;
class MotionMessage;
class ImuDevice;
//class FilteringAlgorithm;

//struct VehicleVelocity
//{
//    double x_move;//according to vehicle unit in m/s
//    double y_move;
//    double z_rotate;//unit is rad/s
//};

//struct VehiclePosition
//{
//    double x;//m
//    double y;//m
//    double rotate;//unit is rad
//};
/**
 * @brief The VehicleJogControl class agv手动控制类
 */
class VehicleJogControl
{
public:
    VehicleJogControl(QString filePathIn,MotionMessage* motionMessageIn,int robotIdIn,double deltaTimeIn);

public:
    int getVehicleMovingCaligrateInfo(struct VehicleMovingCaligrateInfo &infoOut);
    int setCalibrateWheelDiameter(double idealMovedLength,double actualMovedLength);
    int setCalibrateWheelDistance(double idealMovedAngle,double actualMovedAngle);

    int getImuDeviceData(ImuDataInfo &dataOut);
    int getVehicleKinematicDimension(struct VehicleKinematicDimension &infoOut);
    int setVehicleKinematicDimension(struct VehicleKinematicDimension infoIn,int &returnFlag);
    int getVehicleJogControlParameter(struct VehicleJogControlParameter &infoOut);
    int setVehicleJogControlParameter(struct VehicleJogControlParameter infoIn,int &returnFlag);


    int setSteeringControlKp(double kpIn, double maxSteerAdjustIn);
    int setDebugFlag(int flagIn);
    int setJogSafeRatio(double ratioIn);
    int resetAll();
    int stop();
    int startRun();
    bool isInitialOk();
    int vehicleJog(double xMove, double yMove, double rotate);
    int vehicleAuto(double xMove, double yMove, double rotate);
    int setRosVelCommand(double xMove, double yMove, double rotate);
//    int calculateRosVelCommandSmooth();
    int getVehicleVelCommand(bool isJogCommand,double &xMove, double &yMove, double &rotate);
    /**
     * @brief setVirtualLineTrack_feedfowardOutput 设置前馈
     * @param feedforward_x
     * @param feedforward_y
     * @param feedforward_r
     * @return
     */
    int setVirtualLineTrack_feedfowardOutput(double feedforward_xIn, double feedforward_yIn
                  , double feedforward_rIn);
    int moveLoop(QVector<double> &wheelVel, QVector<double> &steeringAngleOut);
    int stopVel(QVector<double> &wheelVel);
    int getWheelData(std::vector<double>& wheelMoveSpeed,std::vector<double>& wheelPathLength);//not used
    int getOdometer(VehicleVelocity& vehicleVelFeedbackOut,
                    VehiclePosition& vehiclePositionFeedbackOut);
    int odometerCalculationLoop(QVector<double> wheelVelFeedback, QVector<double> wheelPosFeedback, QVector<double> steeringAngle);

private:
    int initialSteeringAxis();
    int moveLoop_steeringSmooth(VehicleVelocity vehicleVelCommand_safe, QVector<double> &wheelVel, QVector<double> &steeringAngleOut);
    int jogFitteringLoop();
    int recordWheelFeedback(QVector<double> wheelVelFeedbackIn, QVector<double> wheelPosFeedbackIn, QVector<double> stteringAngleIn);
    int readConfigFile(QString filePathIn, QString &readComent);
    int writeConfigFile(QString filePathIn,double correctionFactor_wheelDiameterIn,double correctionFactor_wheelDistanceIn,
                        double correctionFactor_wheelDistance_heightIn);
    int speedLimitCheck(QVector<double> wheelVel);
    double getTimeInterval(struct timespec& lastTime);//return S
    void addMsg(int messageLevel, std::string componentName, std::string messageType
                , int messageCode, int robotIdIn, double parameter1=0, double parameter2=0,
                int parameter3=0, int parameter4=0,QString comment="");

private:
    QString configFilePath;
    int robotId;
    MotionMessage * motionMessage;
    bool initialFlag;
    E_VEHICLE_MODE mode;
    E_VEHICLE_TYPE vehicletype;
    VehicleVelocity vehicleVelJogCommand;
    VehicleVelocity vehicleVelJogCommand_beforeFilter;
    VehicleVelocity vehicleVelAutoCommand;
    VehicleVelocity vehicleVelRosCommand;//ros_vel_cmd
    double vehicleRearRunSpeedLimit;//m/s
    VehicleVelocity vehicleMaxVel;
    double wheelDiameter;
    double correctionFactor_wheelDiameter;//轮直径修正系数
    double wheelDistance;
    double correctionFactor_wheelDistance;
    double wheelDistance_height;
    double correctionFactor_wheelDistance_height;
    double wheelMaxSpeed;
    double jogAcceleration_rotate;//手动加速平滑滤波
    double jogAcceleration_line;//手动加速平滑滤波
    double filter_maxVel_line;
    double filter_maxVel_rotate;
    double filter_maxAcc_line;
    double filter_maxAcc_rotate;
    double filter_decel_factor;

    VehicleKinematics* vehicleKinematic;
    E_VEHICLE_STATUS status;

    VehicleVelocity vehicleVelFeedback;
    VehiclePosition vehiclePositionFeedback;
    struct timespec timeStamp;
    bool timeStampInitialed;

    double wheelVelFeedback[100];//unit:degree/s
    double wheelPosFeedback[100];//unit:degree
    double steeringAngleFeedback[D_STEERING_MAX_COUNT];//单舵轮或双舵轮转向角度

//    FilteringAlgorithm* filtering_odom_vx;
//    FilteringAlgorithm* filtering_odom_vy;
//    FilteringAlgorithm* filtering_odom_vr;
    double jogSafeRatio;

    FilteringAlgorithm* filtering_safeRatio;//停的太猛了，需要左滤波
    FilteringAlgorithm* filtering_jog_x;//需要内部做线程保护
    FilteringAlgorithm* filtering_jog_y;
    FilteringAlgorithm* filtering_jog_r;
    VehicleVelocitySmoother* velocitySmoother;

    double steeringAngleOffset_wait;//转向偏差角，大于该值就要做过渡等待　。unit degree
    double steeringMaxSpeed;
    double steeringMaxAcceration;
    AxisObject* steeringAxis;//速度比例轴
    double deltaTime;
    bool isInSteeringSmooth;
    double steeringKp;//舵轮转角控制比例系数
    double maxSteerAdjust;
    ImuDevice *imuDevice;


};

#endif // VEHICLEJOGCONTROL_H
